package de.fh.ka.as.car.routing;

import java.util.Date;

import android.graphics.PointF;
import android.util.Log;
import de.fh.ka.as.car.drivecontrol.model.Control;
import de.fh.ka.as.car.hardware.Car;
import de.fh.ka.as.car.hardware.SpeedLookup;
import de.fh.ka.as.car.sensor.AndroidSensor.SensorType;

public class DriveForwardCommand extends RouteCommand {

	public static final int STATE_ACCELERATE = 0;
	public static final int STATE_DRIVE = 1;
	public static final int STATE_ROLLOUT = 2;

	/**
	 * driving distance in cm
	 */
	private double distance;

	/**
	 * distance completed in cm
	 */
	private double distanceDone = 0;

	/**
	 * how many cm should the rollout state last (rollout = no acceleration)
	 */
	private final double distanceRollout = 20;
	/**
	 * if useSpeedometer = false, after how many ms should rollout phase end
	 */
	private final double timeRollout = 2 * 1000; // 2s

	private final boolean useSpeedometer = false;
	/**
	 * if useSpeedometer = false, manual acceleration time
	 */
	private final long timeAccelerating = 2 * 1000; // 2s

	/**
	 * Speed
	 */
	private double driveSpeed = 0;

	/**
	 * @param start
	 *           start point
	 * @param end
	 *           end point
	 * @param distance
	 *           in cm
	 */
	public DriveForwardCommand(PointF start, PointF end, double distance) {
		super(start, end);
		this.distance = distance;
	}

	@Override
	public void execute(Car car, Control control) {
		// first run
		Date timeNow = new Date(System.currentTimeMillis());
		long timeDelta = 0;
		if (lastTime == null) {
			lastTime = new Date(System.currentTimeMillis());
		} else {
			timeDelta = timeNow.getTime() - lastTime.getTime();
		}
		// change State
		if (state != nextState)
			changeState(car, control);

		switch (state) {
			case STATE_START:
				changeToState(STATE_ACCELERATE);
				break;

			case STATE_ACCELERATE:
				// accelerate to a kmh power

				if (useSpeedometer) {
					// check for the wanted speed
					driveSpeed = car.getSpeed();
					if (Math.abs(driveSpeed - SpeedLookup.speedForward) < 0.1
							| SpeedLookup.speedForward < driveSpeed) {
						car.disableSensor(SensorType.Speedometer);
						changeToState(STATE_DRIVE);
					}
				} else {
					// calc current drive speed
					double appliedAcceleration = SpeedLookup.accelerationForward * ((double) timeDelta / 1000);
					driveSpeed += appliedAcceleration * 3600 / 1000;

					// wait till acceleration time is hit
					if (timeAccelerating < timeNow.getTime() - timeStateStarted.getTime())
						changeToState(STATE_DRIVE);
				}
				// update distance
				distanceDone += (driveSpeed / 3600 * 1000 * 100) * ((double) timeDelta / 1000);
				control.setSpeed(SpeedLookup.power[SpeedLookup.DRIVE_ACCELERATE]);

				// check traveled path
				if (distanceDone >= distance)
					changeToState(STATE_FINISHED);
				else if (distanceDone + distanceRollout >= distance)
					changeToState(STATE_ROLLOUT);
				break;

			case STATE_DRIVE:
				// update distance
				distanceDone += (driveSpeed / 3600 * 1000 * 100) * ((double) timeDelta / 1000);
				control.setSpeed(SpeedLookup.power[SpeedLookup.DRIVE_ACCELERATE_CONSTANT]);

				// check travelled path
				if (distanceDone >= distance)
					changeToState(STATE_FINISHED);
				else if (distanceDone + distanceRollout >= distance)
					changeToState(STATE_ROLLOUT);
				break;

			case STATE_ROLLOUT:
				// update distance
				// distanceDone += (driveSpeed / 3600 * 1000) * ((double) timeDelta / 1000);

				// TODO: rollout deceleration and travel distance calculation

				// check traveled path or time
				if (distanceDone >= distance | timeRollout > timeNow.getTime() - timeStateStarted.getTime())
					changeToState(STATE_FINISHED);
				break;
		}
		lastTime = timeNow;
		Log.i("Route", String.format("timeDelta(ms): %s, speed(kmh): %f", "" + timeDelta, driveSpeed));
	}

	private void changeState(Car car, Control control) {
		timeStateStarted = new Date(System.currentTimeMillis());
		switch (nextState) {
			case STATE_START:
				state = nextState;

				// enable sensors and reset speed (in sensor)
				car.enableSensor(SensorType.Speedometer);
				car.setSpeed(0);
				control.setSteering(0);
				control.setSpeed(0);
				break;
			case STATE_ACCELERATE:
				state = nextState;

				control.setSpeed(SpeedLookup.power[SpeedLookup.DRIVE_ACCELERATE]);
				break;
			case STATE_DRIVE:
				state = nextState;

				// set constant drive speed for cruising
				control.setSpeed(SpeedLookup.power[SpeedLookup.DRIVE_ACCELERATE_CONSTANT]);
				if (useSpeedometer)
					driveSpeed = car.getSpeed();
				// driveSpeed = SpeedLookup.speedForward;
				else
					driveSpeed = SpeedLookup.speedForward;
				break;
			case STATE_ROLLOUT:
				state = nextState;
				control.setSpeed(0);
				break;
			case STATE_FINISHED:
				state = nextState;
				control.setSpeed(0);
				break;
		}

	}

	@Override
	public String toString() {
		return String.format("%s, type: %s, distance: %f cm, distance-done: %f cm", super.toString(),
				"DriveForwardCommand", distance, distanceDone);
	}

}
